#pragma once
#include <Eigen/Dense>

namespace demo_cpp_tf {

    class SimpleKalmanFilter 
    {
        public:
            /**
             * @brief 构造函数
             * @param state_dim 状态向量维度
             * @param measure_dim 观测向量维度
             */
            SimpleKalmanFilter(int state_dim, int measure_dim);

            /**
             * @brief 初始化滤波器状态
             * @param x0 初始状态向量
             */
            void init(const Eigen::VectorXd& x0);

            /**
             * @brief 预测步骤（支持非线性模式）
             * @param dt 时间间隔（秒）
             * @param u 控制输入向量（暂未使用，保留接口）
             */
            void predict(double dt, const Eigen::VectorXd& u = Eigen::VectorXd());

            /**
             * @brief 更新步骤（使用自定义观测矩阵）
             * @param z 观测向量
             * @param H 观测矩阵
             */
            void update(const Eigen::VectorXd& z, const Eigen::MatrixXd& H);

            /**
             * @brief 设置状态转移矩阵
             * @param F 新的状态转移矩阵
             * @throws std::invalid_argument 矩阵维度不匹配
             */
            void setTransitionMatrix(const Eigen::MatrixXd& F);

            /**
             * @brief 设置过程噪声协方差矩阵
             * @param Q 新的过程噪声矩阵
             * @throws std::invalid_argument 矩阵维度不匹配
             */
            void setProcessNoiseCov(const Eigen::MatrixXd& Q);

            /**
             * @brief 启用/禁用非线性预测模式
             * @param enable true启用非线性模式，false使用线性模式
             */
            void enableNonlinearMode(bool enable);

            /**
             * @brief 获取当前状态估计
             * @return 状态向量副本
             */
            Eigen::VectorXd getState() const { return x_; }

        private:
            int n_; // 状态维度
            int m_; // 观测维度
            bool is_nonlinear_; // 非线性模式标志
            
            // 状态相关
            Eigen::VectorXd x_; // 状态向量(x, y, z, vx, vy, vz, ω)
            Eigen::MatrixXd P_; // 协方差矩阵
            // 系统模型
            Eigen::MatrixXd F_; // 状态转移矩阵
            Eigen::MatrixXd Q_; // 过程噪声协方差
            // 观测模型
            Eigen::MatrixXd H_; // 观测矩阵
            Eigen::MatrixXd R_; // 观测噪声协方差
            // 计算中间量
            Eigen::MatrixXd K_; // 卡尔曼增益
    };

} // namespace demo_cpp_tf